/********************************************************************
	created:	2008/10/02
	created:	2:10:2008   1:44
	filename: 	h:\SimoneEngine\qianye\src\qylib\math\qyMatrix3.h
	author:		Xiong Jiayu
	copyright:  
	purpose:	
*********************************************************************/
#pragma once

#ifndef __QYMATRIX3_H__
#define __QYMATRIX3_H__

#include "../common/qyCommon.h"
#include "qyVector3.h"
#include <memory.h>

/*
===========================================
struct Matrix3
===========================================
*/
struct Matrix3
{
public:
	Vector3				mat[3];

public:

						Matrix3( void );
	explicit			Matrix3( const Vector3 &x, const Vector3 &y, const Vector3 &z );
	explicit			Matrix3( const float xx, const float xy, const float xz, 
								const float yx, const float yy, const float yz, 
								const float zx, const float zy, const float zz );
	explicit			Matrix3( const float src[3][3] );

	const Vector3 &		operator[]( const int index ) const;
	Vector3 &			operator[]( const int index );

	Matrix3				operator-() const;
	Matrix3				operator*( const float a ) const;
	Vector3				operator*( const Vector3 &v ) const;
	Matrix3				operator*( const Matrix3 &m ) const;

	Vector3				operator/( const Vector3 &v ) const;
	Matrix3				operator/( const Matrix3 &m ) const;

	Matrix3				operator+( const Matrix3 &m ) const;
	Matrix3 &			operator+=( const Matrix3 &m );
	Matrix3				operator-( const Matrix3 &m ) const;
	Matrix3	&			operator-=( const Matrix3 &m );
	Matrix3	&			operator*=( const Matrix3 &m );
	Matrix3 &			operator*=( const float a );

	friend Matrix3		operator*( const float a, const Matrix3 &m );
	friend Vector3		operator*( const Vector3 &v, const Matrix3 &m );
	friend Vector3 &	operator*=( Vector3 &v, const Matrix3 &m );

	bool				Compare( const Matrix3 & m ) const;
	bool				Compare( const Matrix3 & m, const float epsilon ) const;
	bool				operator==( const Matrix3 &m ) const;
	bool				operator!=( const Matrix3 &m ) const;

	void				SetupRotate( const Vector3 &axis, const float angle );

	void				Identity( void );
	Matrix3				Inverse( void ) const;
	bool				InverseSelf( void );
	Matrix3				Transpose( void ) const;	// returns transpose
	Matrix3 &			TransposeSelf( void );
	void				RotateAbsolute( int axis, float degrees );
	void				RotateRelative( int axis, float	degrees );
	void				RotateAribitrary( const Vector3 &axis, float degrees );

	void				FromQuat( const float q[4] );
	void				ToQuat( float q[4] ) const;
	bool				ToEulerAnglesXYZ( float& rfXAngle, float& rfYAngle,	float& rfZAngle ) const;
};

const Matrix3 mat3_zero( 0, 0, 0 , 0, 0, 0 , 0, 0, 0 );
const Matrix3 mat3_identity( 1, 0, 0 , 0, 1, 0 , 0, 0, 1 );
/*
===========================================
class Matrix3 inline
===========================================
*/                      
QY_INLINE Matrix3::Matrix3( void )
{
}

QY_INLINE Matrix3::Matrix3(const Vector3 &x, const Vector3 &y, const Vector3 &z)
{
	memcpy( mat, &x, sizeof( float ) * 3 );
	memcpy( mat + 1, &y, sizeof( float ) * 3 );
	memcpy( mat + 2, &z, sizeof( float ) * 3 );
}

QY_INLINE Matrix3::Matrix3(const float xx, const float xy, const float xz, 
						   const float yx, const float yy, const float yz, 
						   const float zx, const float zy, const float zz )
{
	mat[0][0] = xx; mat[0][1] = xy; mat[0][2] = xz;
	mat[1][0] = yx; mat[1][1] = yy; mat[1][2] = yz;
	mat[2][0] = zx; mat[2][1] = zy; mat[2][2] = zz;
}

QY_INLINE Matrix3::Matrix3(const float src[3][3])
{
	memcpy( mat, src, sizeof( float ) * 9 );
}

QY_INLINE const Vector3 & Matrix3::operator []( const int index ) const
{
	return mat[index];
}

QY_INLINE Vector3 & Matrix3::operator []( const int index )
{
	return mat[index];
}

QY_INLINE Matrix3 Matrix3::operator -( void ) const
{
	return Matrix3( -mat[0][0], -mat[0][1], -mat[0][2],
		-mat[1][0], -mat[1][1], -mat[1][2],
		-mat[2][0], -mat[2][1], -mat[2][2] );
}

QY_INLINE Matrix3 Matrix3::operator *( const float a ) const
{
	return Matrix3( mat[0][0] * a, mat[0][1] * a, mat[0][2] * a, mat[1][0] * a, mat[1][1] * a, mat[1][2] * a, mat[2][0] * a, mat[2][1] * a, mat[2][2] * a );
}

QY_INLINE Vector3 Matrix3::operator *( const Vector3 &v ) const
{
	return Vector3( mat[0].x * v.x + mat[1].x * v.y + mat[2].x * v.z,
		mat[0].y * v.x + mat[1].y * v.y + mat[2].y * v.z,
		mat[0].z * v.x + mat[1].z * v.y + mat[2].z * v.z
		);
}

QY_INLINE Matrix3 Matrix3::operator *( const Matrix3 &m ) const
{
	int i, j;
	const float *m1Ptr, *m2Ptr;
	float *dstPtr;
	Matrix3 dst;

	m1Ptr = reinterpret_cast<const float *>(this);
	m2Ptr = reinterpret_cast<const float *>(&m);
	dstPtr = reinterpret_cast<float *>(&dst);

	for ( i = 0; i < 3; i++ ) {
		for ( j = 0; j < 3; j++ ) {
			*dstPtr = m1Ptr[0] * m2Ptr[ 0 * 3 + j ]
			+ m1Ptr[1] * m2Ptr[ 1 * 3 + j ]
			+ m1Ptr[2] * m2Ptr[ 2 * 3 + j ];
			dstPtr++;
		}
		m1Ptr += 3;
	}
	return dst; 
}

QY_INLINE Vector3 Matrix3::operator /( const Vector3 &v ) const
{
	return Vector3( mat[0].x * v.x + mat[0].y * v.y + mat[0].z * v.z,
		mat[1].x * v.x + mat[1].y * v.y + mat[1].z * v.z,
		mat[2].x * v.x + mat[2].y * v.y + mat[2].z * v.z 
		);
}

QY_INLINE Matrix3 Matrix3::operator /( const Matrix3 &m ) const
{
	Matrix3		dst;

	dst[0].x = mat[0].x * m.mat[0].x + mat[0].y * m.mat[0].y + mat[0].z * m.mat[0].z;
	dst[0].y = mat[0].x * m.mat[1].x + mat[0].y * m.mat[1].y + mat[0].z * m.mat[1].z;
	dst[0].z = mat[0].x * m.mat[2].x + mat[0].y * m.mat[2].y + mat[0].z * m.mat[2].z;

	dst[1].x = mat[1].x * m.mat[0].x + mat[1].y * m.mat[0].y + mat[1].z * m.mat[0].z;
	dst[1].y = mat[1].x * m.mat[1].x + mat[1].y * m.mat[1].y + mat[1].z * m.mat[1].z;
	dst[1].z = mat[1].x * m.mat[2].x + mat[1].y * m.mat[2].y + mat[1].z * m.mat[2].z;

	dst[2].x = mat[2].x * m.mat[0].x + mat[2].y * m.mat[0].y + mat[2].z * m.mat[0].z;
	dst[2].y = mat[2].x * m.mat[1].x + mat[2].y * m.mat[1].y + mat[2].z * m.mat[1].z;
	dst[2].z = mat[2].x * m.mat[2].x + mat[2].y * m.mat[2].y + mat[2].z * m.mat[2].z;

	return( dst );
}

QY_INLINE Matrix3 Matrix3::operator +( const Matrix3 &m ) const
{
	return Matrix3( mat[0][0] + m.mat[0][0], mat[0][1] + m.mat[0][1], mat[0][2] + m.mat[0][2], 
		mat[1][0] + m.mat[1][0], mat[1][1] + m.mat[1][1], mat[1][2] + m.mat[1][2],
		mat[2][0] + m.mat[2][0], mat[2][1] + m.mat[2][1], mat[2][2] + m.mat[2][2] 
	);
}

QY_INLINE Matrix3 & Matrix3::operator +=( const Matrix3 &m )
{
	mat[0][0] += m.mat[0][0]; mat[0][1] += m.mat[0][1]; mat[0][2] += m.mat[0][2];
	mat[1][0] += m.mat[1][0]; mat[1][1] += m.mat[1][1]; mat[1][2] += m.mat[1][2];
	mat[2][0] += m.mat[2][0]; mat[2][1] += m.mat[2][1]; mat[2][2] += m.mat[2][2];
	return (*this);
}

QY_INLINE Matrix3 Matrix3::operator -(const Matrix3 &m) const
{
	return Matrix3( mat[0][0] - m.mat[0][0], mat[0][1] - m.mat[0][1], mat[0][2] - m.mat[0][2], 
		mat[1][0] - m.mat[1][0], mat[1][1] - m.mat[1][1], mat[1][2] - m.mat[1][2],
		mat[2][0] - m.mat[2][0], mat[2][1] - m.mat[2][1], mat[2][2] - m.mat[2][2] 
	);
}

QY_INLINE Matrix3 & Matrix3::operator -=( const Matrix3 &m )
{
	mat[0][0] -= m.mat[0][0]; mat[0][1] += m.mat[0][1]; mat[0][2] += m.mat[0][2];
	mat[1][0] -= m.mat[1][0]; mat[1][1] += m.mat[1][1]; mat[1][2] += m.mat[1][2];
	mat[2][0] -= m.mat[2][0]; mat[2][1] += m.mat[2][1]; mat[2][2] += m.mat[2][2];
	return (*this);
}

QY_INLINE Matrix3 & Matrix3::operator *=( const Matrix3 &m )
{
	int i, j;
	const float *m2Ptr;
	float *m1Ptr, dst[3];

	m1Ptr = reinterpret_cast<float *>(this);
	m2Ptr = reinterpret_cast<const float *>(&m);

	for ( i = 0; i < 3; i++ ) {
		for ( j = 0; j < 3; j++ ) {
			dst[j]  = m1Ptr[0] * m2Ptr[ 0 * 3 + j ]
			+ m1Ptr[1] * m2Ptr[ 1 * 3 + j ]
			+ m1Ptr[2] * m2Ptr[ 2 * 3 + j ];
		}
		m1Ptr[0] = dst[0]; m1Ptr[1] = dst[1]; m1Ptr[2] = dst[2];
		m1Ptr += 3;
	}
	return (*this);
}

QY_INLINE Matrix3 & Matrix3::operator *=( const float a )
{
	mat[0][0] *= a; mat[0][1] *= a; mat[0][2] *= a;
	mat[1][0] *= a; mat[1][1] *= a; mat[1][2] *= a;
	mat[2][0] *= a; mat[2][1] *= a; mat[2][2] *= a;
	return (*this);
}

QY_INLINE Vector3 operator*( const Vector3 &v, const Matrix3 &m )
{
	return Vector3( m[0].x * v.x + m[1].x * v.y + m[2].x * v.z,
		m[0].y * v.x + m[1].y * v.y + m[2].y * v.z,
		m[0].z * v.x + m[1].z * v.y + m[2].z * v.z 
		);
}

QY_INLINE Matrix3 operator*( const float a, const Matrix3 &m )
{
	return Matrix3( m.mat[0][0] * a, m.mat[0][1] * a, m.mat[0][2] * a, 
		m.mat[1][0] * a, m.mat[1][1] * a, m.mat[1][2] * a, 
		m.mat[2][0] * a, m.mat[2][1] * a,m. mat[2][2] * a 
		);
}

QY_INLINE Vector3 & operator*=( Vector3 &v, const Matrix3 &m )
{
	float x = m[ 0 ].x * v.x + m[ 1 ].x * v.y + m[ 2 ].x * v.z;
	float y = m[ 0 ].y * v.x + m[ 1 ].y * v.y + m[ 2 ].y * v.z;
	v.z = m[ 0 ].z * v.x + m[ 1 ].z * v.y + m[ 2 ].z * v.z;
	v.x = x;
	v.y = y;
	return v;
}

QY_INLINE bool Matrix3::Compare(const Matrix3 &m) const
{
	return true;
}

QY_INLINE void Matrix3::SetupRotate(const Vector3 &axis, const float angle)
{
	assert(fabs(axis*axis - 1.0f) < .01f);

	// Get sin and cosine of rotation angle

	float	s, c;
	s = sinf( angle );
	c = cosf( angle );

	float	a = 1.0f - c;
	float	ax = a * axis.x;
	float	ay = a * axis.y;
	float	az = a * axis.z;

	mat[0][0] = ax * axis.x + c;
	mat[0][1] = ax * axis.y + axis.z * s;
	mat[0][2] = ax * axis.z - axis.y * s;

	mat[1][0] = ay * axis.x - axis.z * s;
	mat[1][1] = ay * axis.y + c;
	mat[1][2] = ay * axis.z + axis.x * s;

	mat[2][0] = az * axis.x + axis.y * s;
	mat[2][1] = az * axis.y - axis.x * s;
	mat[2][2] = az * axis.z + c;
}

QY_INLINE void Matrix3::Identity()
{
	*this = mat3_identity;
}

QY_INLINE Matrix3 Matrix3::Inverse() const
{
	Matrix3 invMat;
	invMat = *this;
	invMat.InverseSelf();
	return invMat;
}

QY_INLINE bool Matrix3::InverseSelf()
{
	Matrix3 inverse;
	double det, invDet;

	inverse[0][0] = mat[1][1] * mat[2][2] - mat[1][2] * mat[2][1];
	inverse[1][0] = mat[1][2] * mat[2][0] - mat[1][0] * mat[2][2];
	inverse[2][0] = mat[1][0] * mat[2][1] - mat[1][1] * mat[2][0];

	det = mat[0][0] * inverse[0][0] + mat[0][1] * inverse[1][0] + mat[0][2] * inverse[2][0];

	invDet = 1.0f / det;

	inverse[0][1] = mat[0][2] * mat[2][1] - mat[0][1] * mat[2][2];
	inverse[0][2] = mat[0][1] * mat[1][2] - mat[0][2] * mat[1][1];
	inverse[1][1] = mat[0][0] * mat[2][2] - mat[0][2] * mat[2][0];
	inverse[1][2] = mat[0][2] * mat[1][0] - mat[0][0] * mat[1][2];
	inverse[2][1] = mat[0][1] * mat[2][0] - mat[0][0] * mat[2][1];
	inverse[2][2] = mat[0][0] * mat[1][1] - mat[0][1] * mat[1][0];

	mat[0][0] = (float)( inverse[0][0] * invDet );
	mat[0][1] = (float)( inverse[0][1] * invDet );
	mat[0][2] = (float)( inverse[0][2] * invDet );

	mat[1][0] = (float)( inverse[1][0] * invDet );
	mat[1][1] = (float)( inverse[1][1] * invDet );
	mat[1][2] = (float)( inverse[1][2] * invDet );

	mat[2][0] = (float)( inverse[2][0] * invDet );
	mat[2][1] = (float)( inverse[2][1] * invDet );
	mat[2][2] = (float)( inverse[2][2] * invDet );

	return true;
}

QY_INLINE void Matrix3::RotateAbsolute(int axis, float degrees)
{
	Matrix3 rotation;
	float sinVal, cosVal;
	float angle = degrees * cDEG2RAD;
	sinVal = sinf( angle );
	cosVal = cosf( angle );

	rotation.Identity();
	switch ( axis )
	{
	case 0: // x-axis
		rotation.mat[1][1] = cosVal;
		rotation.mat[1][2] = sinVal;
		rotation.mat[2][1] = -sinVal;
		rotation.mat[2][2] = cosVal;
		break;
	case 1: // y-axis
		rotation.mat[0][0] = cosVal;
		rotation.mat[0][2] = -sinVal;
		rotation.mat[2][0] = sinVal;
		rotation.mat[2][2] = cosVal;
		break;
	case 2: // z-axis
		rotation.mat[0][0] = cosVal;
		rotation.mat[0][1] = sinVal;
		rotation.mat[1][0] = -sinVal;
		rotation.mat[1][1] = cosVal;
		break;
	default:
		assert( !"Matrix3::RotationAbsolute axis must be 0, 1 or 2" );
		break;
	}
	*this *= rotation;
}

QY_INLINE void Matrix3::RotateRelative(int axis, float degrees)
{
	Matrix3 rotation;
	float sinVal, cosVal;
	float angle = degrees * cDEG2RAD;
	sinVal = ::sinf( angle );
	cosVal = ::cosf( angle );

	rotation.Identity();
	switch ( axis )
	{
	case 0: // x-axis
		rotation.mat[1][1] = cosVal;
		rotation.mat[1][2] = sinVal;
		rotation.mat[2][1] = -sinVal;
		rotation.mat[2][2] = cosVal;
		break;
	case 1: // y-axis
		rotation.mat[0][0] = cosVal;
		rotation.mat[0][2] = -sinVal;
		rotation.mat[2][0] = sinVal;
		rotation.mat[2][2] = cosVal;
		break;
	case 2:
		rotation.mat[0][0] = cosVal;
		rotation.mat[0][1] = sinVal;
		rotation.mat[1][0] = -sinVal;
		rotation.mat[1][1] = cosVal;
		break;
	default:
		assert( !"Matrix3::RotationRelative axis must be 0, 1 or 2" );
		break;
	}
	*this = rotation * ( *this );
}

QY_INLINE void Matrix3::RotateAribitrary(const Vector3 &axis, float degrees)
{
	float	sinVal, cosVal;
	float	angle = degrees * cDEG2RAD;
	sinVal = ::sinf( angle );
	cosVal = ::cosf( angle );
	float	d = ::sqrt( axis[1] * axis[1] + axis[2] * axis[2] );

	if (axis.Compare(Vector3(1, 0, 0)))
	{
		RotateAbsolute(0, degrees);
		return;
	}
	if (axis.Compare(Vector3(-1, 0, 0)))
	{
		RotateAbsolute(0, -degrees);
		return;
	}
	if (axis.Compare(Vector3(0, 1, 0)))
	{
		RotateAbsolute(1, degrees);
		return;
	}
	if (axis.Compare(Vector3(0, -1, 0)))
	{
		RotateAbsolute(1, -degrees);
		return;
	}
	if (axis.Compare(Vector3(0, 0, 1)))
	{
		RotateAbsolute(2, degrees);
		return;
	}
	if (axis.Compare(Vector3(0, 0, -1)))
	{
		RotateAbsolute(2, -degrees);
		return;
	}

	Matrix3 rotationX( 1, 0, 0, 0, axis[2]/d, axis[1]/d, 0, -axis[1]/d, axis[2]/d );
	Matrix3 rotationY( d, 0, axis[0], 0, 1, 0, -axis[0], 0, d );
	Matrix3 rotationZ( cosVal, sinVal, 0, -sinVal, cosVal, 0, 0, 0, 1 );
	Matrix3 rotationXinv = rotationX.Inverse();
	Matrix3 rotationYinv = rotationY.Inverse();
	Matrix3 tmpMat;

	tmpMat = rotationYinv * rotationXinv;
	tmpMat = rotationZ * tmpMat;
	tmpMat = rotationY * tmpMat;
	tmpMat = rotationX * tmpMat;
	*this = *this * tmpMat;
}

//-----------------------------------------------------------------------
QY_INLINE Matrix3 Matrix3::Transpose() const
{
	return Matrix3(	mat[0][0], mat[1][0], mat[2][0],
		mat[0][1], mat[1][1], mat[2][1],
		mat[0][2], mat[1][2], mat[2][2] ); 
}

//-----------------------------------------------------------------------
QY_INLINE Matrix3 &Matrix3::TransposeSelf()
{
	float tmp0, tmp1, tmp2;

	tmp0 = mat[0][1];
	mat[0][1] = mat[1][0];
	mat[1][0] = tmp0;
	tmp1 = mat[0][2];
	mat[0][2] = mat[2][0];
	mat[2][0] = tmp1;
	tmp2 = mat[1][2];
	mat[1][2] = mat[2][1];
	mat[2][1] = tmp2;

	return (*this);
}

//-----------------------------------------------------------------------
QY_INLINE void Matrix3::FromQuat( const float q[4] )
{
	float	wx, wy, wz;
	float	xx, yy, yz;
	float	xy, xz, zz;
	float	x2, y2, z2;

	x2 = q[0] + q[0];
	y2 = q[1] + q[1];
	z2 = q[2] + q[2];

	xx = q[1] * x2;
	xy = q[1] * y2;
	xz = q[1] * z2;

	yy = q[2] * y2;
	yz = q[2] * z2;
	zz = q[2] * z2;

	wx = q[3] * x2;
	wy = q[3] * y2;
	wz = q[3] * z2;

	mat[ 0 ][ 0 ] = 1.0f - ( yy + zz );
	mat[ 1 ][ 0 ] = xy - wz;
	mat[ 2 ][ 0 ] = xz + wy;

	mat[ 0 ][ 1 ] = xy + wz;
	mat[ 1 ][ 1 ] = 1.0f - ( xx + zz );
	mat[ 2 ][ 1 ] = yz - wx;

	mat[ 0 ][ 2 ] = xz - wy;
	mat[ 1 ][ 2 ] = yz + wx;
	mat[ 2 ][ 2 ] = 1.0f - ( xx + yy );
}

//-----------------------------------------------------------------------
QY_INLINE bool Matrix3::ToEulerAnglesXYZ( float& rfXAngle, float& rfYAngle, float& rfZAngle ) const
{
	rfYAngle = -asinf(mat[0][2]);
	if ( rfYAngle < 0.5f*cQY_PI )
	{
		if ( rfYAngle > -0.5f*cQY_PI )
		{
			rfXAngle = -atan2f(-mat[1][2],mat[2][2]);
			rfZAngle = -atan2f(-mat[0][1],mat[0][0]);
			return true;
		}
		else
		{
			// WARNING.  Not a unique solution.
			float fRmY = atan2f(mat[1][0],mat[1][1]);
			rfZAngle = 0.0f;  // any angle works
			rfXAngle = fRmY - rfZAngle;
			return false;
		}
	}
	else
	{
		// WARNING.  Not a unique solution.
		float fRpY = atan2f(mat[1][0],mat[1][1]);
		rfZAngle = 0.0f;  // any angle works
		rfXAngle = rfZAngle - fRpY;
		return false;
	}
}


#endif

